package com.itistimetothink.cjetso.utilities;

import java.util.Timer;

public class SensorFusion {

	public static final int TIME_CONSTANT = 30;
	public static final float FILTER_COEFFICIENT = 0.98f;
	private Timer fuseTimer = new Timer();
	// orientation angles from accel and magnet
	private float[] accMagOrientation = new float[3];

	// final orientation angles from sensor fusion
	private float[] fusedOrientation = new float[3];

	// accelerometer and magnetometer based rotation matrix
	private float[] rotationMatrix = new float[9];
	// orientation angles from gyro matrix
	private float[] gyroOrientation = new float[3];

	private float[] gyroMatrix = new float[9];

	public SensorFusion() {

	}

	public void getRotationMatrixFromOrientation(float[] o, float[] outPut) {
		float[] xM = new float[9];
		float[] yM = new float[9];
		float[] zM = new float[9];

		float sinX = (float) Math.sin(o[1]);
		float cosX = (float) Math.cos(o[1]);
		float sinY = (float) Math.sin(o[2]);
		float cosY = (float) Math.cos(o[2]);
		float sinZ = (float) Math.sin(o[0]);
		float cosZ = (float) Math.cos(o[0]);

		// rotation about x-axis (pitch)
		xM[0] = 1.0f;
		xM[1] = 0.0f;
		xM[2] = 0.0f;
		xM[3] = 0.0f;
		xM[4] = cosX;
		xM[5] = sinX;
		xM[6] = 0.0f;
		xM[7] = -sinX;
		xM[8] = cosX;

		// rotation about y-axis (roll)
		yM[0] = cosY;
		yM[1] = 0.0f;
		yM[2] = sinY;
		yM[3] = 0.0f;
		yM[4] = 1.0f;
		yM[5] = 0.0f;
		yM[6] = -sinY;
		yM[7] = 0.0f;
		yM[8] = cosY;

		// rotation about z-axis (azimuth)
		zM[0] = cosZ;
		zM[1] = sinZ;
		zM[2] = 0.0f;
		zM[3] = -sinZ;
		zM[4] = cosZ;
		zM[5] = 0.0f;
		zM[6] = 0.0f;
		zM[7] = 0.0f;
		zM[8] = 1.0f;

		// rotation order is y, x, z (roll, pitch, azimuth)
		float[] resultMatrix = matrixMultiplication(xM, yM);
		resultMatrix = matrixMultiplication(zM, resultMatrix);
		outPut = resultMatrix;
		//return resultMatrix;
	}

	private float[] matrixMultiplication(float[] A, float[] B) {
		float[] result = new float[9];

		result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
		result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
		result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

		result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
		result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
		result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

		result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
		result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
		result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

		return result;
	}

	public float[] calculateFusedOrientationTask(float[] gyroOrientation,
			float[] accMagOrientation) {

		float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
		this.accMagOrientation = accMagOrientation;
		this.gyroOrientation = gyroOrientation;
		/*
		 * Fix for 179?<--> -179?transition problem: Check whether one of the
		 * two orientation angles (gyro or accMag) is negative while the other
		 * one is positive. If so, add 360?(2 * math.PI) to the negative value,
		 * perform the sensor fusion, and remove the 360?from the result if it
		 * is greater than 180? This stabilizes the output in
		 * positive-to-negative-transition cases.
		 */

		// azimuth
		if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
			fusedOrientation[0] = (float) (FILTER_COEFFICIENT
					* (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff
					* accMagOrientation[0]);
			fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else if (accMagOrientation[0] < -0.5 * Math.PI
				&& gyroOrientation[0] > 0.0) {
			fusedOrientation[0] = (float) (FILTER_COEFFICIENT
					* gyroOrientation[0] + oneMinusCoeff
					* (accMagOrientation[0] + 2.0 * Math.PI));
			fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else {
			fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0]
					+ oneMinusCoeff * accMagOrientation[0];
		}

		// pitch
		if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) {
			fusedOrientation[1] = (float) (FILTER_COEFFICIENT
					* (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff
					* accMagOrientation[1]);
			fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else if (accMagOrientation[1] < -0.5 * Math.PI
				&& gyroOrientation[1] > 0.0) {
			fusedOrientation[1] = (float) (FILTER_COEFFICIENT
					* gyroOrientation[1] + oneMinusCoeff
					* (accMagOrientation[1] + 2.0 * Math.PI));
			fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else {
			fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1]
					+ oneMinusCoeff * accMagOrientation[1];
		}

		// roll
		if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) {
			fusedOrientation[2] = (float) (FILTER_COEFFICIENT
					* (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff
					* accMagOrientation[2]);
			fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else if (accMagOrientation[2] < -0.5 * Math.PI
				&& gyroOrientation[2] > 0.0) {
			fusedOrientation[2] = (float) (FILTER_COEFFICIENT
					* gyroOrientation[2] + oneMinusCoeff
					* (accMagOrientation[2] + 2.0 * Math.PI));
			fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI
					: 0;
		} else {
			fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2]
					+ oneMinusCoeff * accMagOrientation[2];
		}

		// overwrite gyro matrix and orientation with fused orientation
		// to comensate gyro drift
		//getRotationMatrixFromOrientation(fusedOrientation, gyroMatrix);
		System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
		return fusedOrientation;

	}
}
